% solve: argmax log(det X) - tr(X . S) - \lambda \|X\|_1 : X \in S_+^(pxp)
% dual : argmin -log(det (S+U)) - p : \|U\|_infty \leq \lambda
% dual : argmin -log(det W) - p : \|W-S\|_infty \leq \lambda

% input: S = covariance matrix
%        opt.lambda = l1 regularization 
%        opt.diag = diagonal addition (default opt.lambda)
%        opt.maxiter = maximum number of iterations
%        opt.W0 = starting point
function W = inverse_sparse(S, lambda, diag_add, maxiter, W0)
    [p,~] = size(S);
    if (p ~= size(S,2)) 
        error('S must be square');
    end
    
    if nargin < 2
        lambda = 0.01;
    end
    
    if nargin < 3
        diag_add = lambda;
    end
    
    if nargin < 4
        maxiter = 10;
    end
    
    if nargin < 5
        W = S + diag_add*eye(p);
    else
        W = W0;
    end
    
    for iter=1:maxiter
        for j=1:p
            S_j = S(:, j); S_j(j) = [];
            W_jj = W; W_jj(:,j) = []; W_jj(j, :) = [];
            y = solve_column;
            y = [y(1:j-1); W(j,j); y(j:end)];
            W(:,j) = y; W(j, :) = y';
        end
        invW = inv(W);
        duality_gap = trace(invW * S)-p+lambda*sum(sum(abs(invW)));
        display(sprintf('iter=%d gap=%f', iter, duality_gap));
    end
    
    % solve: y = argmin y^T W_jj y : \|y-S_j\|_infty \leq lambda
    function y = solve_column
        problem.H = inv(W_jj);
        problem.f = zeros(p-1,1);
        problem.Aineq = [];
        problem.bineq = [];
        problem.Aeq = [];
        problem.beq = [];
        problem.lb = S_j - lambda;
        problem.ub = S_j + lambda;
        problem.x0 = S_j;
        problem.solver = 'quadprog';
        problem.options = optimset; %('Display', 'off');
        y = quadprog(problem);
    end
end
